#include "user_device.h"
#include "modbus.h"
#include "tim.h"
#include "usart.h"
#include "speed_cntr.h"
#include "device_spi.h"
#include <setjmp.h>
#include <stdint.h>
#include <assert.h>
#include <string.h>
#include "flash_if.h"
#include "crc.h"
#include "stdbool.h"

jmp_buf main_loop_env;
UU16 system_tick = {0};

DevRunParam dev_run_param;
uchar Read_Dev_Param(void);

ushort SystemTickLow()
{
    ushort tick = timer_counter_read(TIMER6);
    return tick;
}

ushort SystemTickHigh()
{
    uchar hi, lo;

    do
    {
        hi = system_tick.UU8[MSB];
        lo = system_tick.UU8[LSB];
    }
    while(hi != system_tick.UU8[MSB]);

    return (ushort)hi << 8 | lo;
}

ulong SystemTick()
{
    ushort hi, lo;
    do
    {
        hi = SystemTickHigh();
        lo = SystemTickLow();
    }
    while(hi != SystemTickHigh());

    return (ulong)(hi << 16 | lo);
}

void Delay_Tick(ushort tick)
{
    ushort t = SystemTickLow();
    while((ushort)(SystemTickLow() - t) < tick);
}

void Delay_Tick_Loop(ulong tick)
{
    ulong t = SystemTick();
    while((ulong)(SystemTick() - t) < tick)
        Process_Main_Loop();
}

void Read_Address(void)
{
    server_address = 0x11;
}

void Write_Dev_Param(void)
{
    fmc_erase_sector_by_address(0x080C0000);
    int16_t offset_param_save[3] = 
    {
        (int16_t)dev_run_param.s_home_step_buffer[S1],
        (int16_t)dev_run_param.s_home_step_buffer[S2],
        (int16_t)dev_run_param.s_home_step_buffer[S3],
    };
    fmc_write_16bit_data(0x080C0000,3,offset_param_save);
    Delay_Tick_Loop(5000);
    Read_Dev_Param();
}

uchar Read_Dev_Param(void)
{
    uint8_t i;
    bool param_valid = false;
    int16_t offset_param_fetch[3] = {0};
    Update_WatchDog();
    fmc_read_16bit_data(0x080C0000,3,offset_param_fetch);
    // 检查数据有效性
    for ( i = 0; i < 3; i++)
    {
        if (offset_param_fetch[i] > 0 && offset_param_fetch[i] != 0xFFFF)
        {
            param_valid = true; // 数据有效
            dev_run_param.s_home_step_buffer[i] = offset_param_fetch[i];
        }
    }
    // 数据无效
    if (!param_valid)
    {
        return false;
    }

    if (dev_run_param.s_home_step_buffer[S1] > srd[S1].reset_step)
    {
        srd[S1].reset_step = dev_run_param.s_home_step_buffer[S1];
    }
    if (dev_run_param.s_home_step_buffer[S2] > srd[S2].reset_step)
    {
        srd[S2].reset_step = dev_run_param.s_home_step_buffer[S2];
    }
    if (dev_run_param.s_home_step_buffer[S3] > srd[S3].reset_step)
    {
        srd[S3].reset_step = dev_run_param.s_home_step_buffer[S3];
    }
    return true;
}

void Device_Init(void)
{

    FLASH_If_Init();
    Read_Address();
    InitMachine();

    // 判断是否可以更新flash参数to电机复位缓存步数
    Read_Dev_Param();
    IO_SPI_SELECT_NONE();

    Drv_Disable(S1);
    Drv_Disable(S2);
    Drv_Disable(S3);
    Drv_Work_Lock(S4);
    //Drv_Work_Full(S1);
    //Drv_Work_Full(S2);
    //Drv_Work_Full(S3);
   // Drv_Work_Full(S4);
    Drv_Set_Decay(S1);
    Drv_Set_Decay(S2);
    Drv_Set_Decay(S3);
    Drv_Set_Decay(S4);

    IO_Set(IO_V1);
    IO_Set(IO_V2);
    IO_Set(IO_V3);
    IO_Set(IO_V4);

//    rw_word.action = SELFTEST;
//    pending_action = 1;

//    rw_word.action = MRESET;
//    pending_action = 1;
}





